/*******************************************************************************
* FILE NAME: user_routines_fast.c <FRC VERSION>
*
* DESCRIPTION:
*  This file is where the user can add their custom code within the framework
*  of the routines below. 
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
*
* OPTIONS:  Interrupts are disabled and not used by default.
*
*******************************************************************************/

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "serial_ports.h"
#include "tracking.h"
#include "camera.h"
#include <stdio.h>
// #include "user_Serialdrv.h"
static unsigned int autostep = 0;
static unsigned int stoptime = 0;
static unsigned int masttime = 0;
static unsigned int scoretime = 0;
static unsigned int loops2 = 0;
static unsigned int loopnumber = 0;
static unsigned int gyro_raw = 0;
static unsigned int correction = 0;
static unsigned int factor = 0;
static unsigned int gyro_time = 0;
/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/

void Drive_To_Light_Far(void) {//wider driving envelope
	if (Get_Tracking_State() == CAMERA_ON_TARGET){
		printf("ON TARGET\r");
		if (TILT_SERVO < 180) {
			printf("tilt\r");
			if (PAN_SERVO < 120) {
				printf("must turn right\r");
				pwm01 = 220;
				pwm02 = 190;
			}
			else if (PAN_SERVO > 128) {
			printf("must turn left\r");
				pwm01 = 190;
				pwm02 = 220;
			}
			else {
				printf("stopped\r");
				pwm01 = 220;
				pwm02 = 220;
			}
		}
		if (TILT_SERVO >= 230 ) {
			autostep++;
		}
	}
	else {
		pwm01 = 127;
		pwm02 = 127;
	}
}

void GetCorrection(void)
{
//	factor = 600 - gyro_raw;//difference from neuteral value
//	correction = correction - factor;//put difference into overall correction

	if (gyro_raw < 600){
		factor = 1;
		correction = correction - factor;
		}
	if (gyro_raw > 606){
		factor = 1;
		correction = correction + factor;
		}
}

void Drive_Straight(void)
{
		
	if (gyro_time < 90){
		if (correction == 0)//if gyro is straight
		{
			pwm01 = 160;//even voltage
			pwm02 = 160;//even voltage
			//printf("moving straight %d \r",autotime);
			//autotime = autotime - 1;
			gyro_time++;
		}
		else if (correction < 0) // if robot moves left
		{
			pwm01 = 180;//speed up left
			pwm02 = 127;//slow down right
			//printf("moving right %d \r",autotime);
			gyro_time++;
			//autotime = autotime - 1;
		}
		else if (correction > 0)// if robot moves right
		{
			pwm01 = 127;//slow down left
			pwm02 = 180;//speed up right
			gyro_time++;	
			//printf("moving left %d \r",autotime);
			//autotime = autotime - 1;
		}
	if (gyro_time >= 90){
		autostep++;
		}
}
}
void Drive_To_Light(void) {
	if (Get_Tracking_State() == CAMERA_ON_TARGET){
		printf("ON TARGET\r");
		if (TILT_SERVO < 235) {
			printf("tilt\r");
			if (PAN_SERVO < 123) {//pan is to left
				printf("must turn right\r");
				pwm01 = 220;//increase right motor
				pwm02 = 190;//increase left motor
			}
			else if (PAN_SERVO > 125) {//pan is to right
			printf("must turn left\r");
				pwm01 = 190;//slow right
				pwm02 = 220;//fast left
			}
			else {
				printf("stopped\r");
				pwm01 = 220;//equal
				pwm02 = 220;//equal
			}
		}
		if (TILT_SERVO >= 235 ) {
			autostep++;//go to next step
		}
	}
	else {
		pwm01 = 127;//dont drive if camera off target
		pwm02 = 127;//dont drive if camera off target
	}
}

/*******
*raise mast
*input loop amt
********/
void mastup(int loops2) {
	if (masttime < loops2){
		pwm03 = 10;//mast is reversed lower=up
		masttime++;//add to counter
		printf("mast%d   %d\r",masttime, loops2);//dbg
	}	
	if (masttime == loops2) {
		pwm03 = 127;//stop mast
		autostep ++;//go to next step
		printf("else%d    %d\r",masttime, loops2);//dbg
	}
}

void stop(int loops) {
	if ( stoptime < loops){
		pwm01 = pwm02 = 127;//stop drive motors
		stoptime++;
	}
	if ( stoptime == loops){
		autostep++;//go to next step
	}
}


void drive(void){
	if (90 > loopnumber) {
		pwm01 = 190;
		pwm02 = 180;
		}
	if (90 == loopnumber){
		autostep++;
	}
	loopnumber++;
}

void score(void) {
	if (scoretime < 2)
	{
		relay5_fwd = 1;//lower wrist
	}
	else if (scoretime < 8)
	{
		relay1_fwd = 1;//open claw
	}
	else if (scoretime == 8)
	{
		autostep++;//go to next step(end)
	}
	scoretime++;
}

/*******************************************************************************
* FUNCTION NAME: InterruptVectorLow
* PURPOSE:       Low priority interrupt vector
* CALLED FROM:   nowhere by default
* ARGUMENTS:     none
* RETURNS:       void
* DO NOT MODIFY OR DELETE THIS FUNCTION 
*******************************************************************************/
#pragma code InterruptVectorLow = LOW_INT_VECTOR
void InterruptVectorLow (void)
{
  _asm
    goto InterruptHandlerLow  /*jump to interrupt routine*/
  _endasm
}


/*******************************************************************************
* FUNCTION NAME: InterruptHandlerLow
* PURPOSE:       Low priority interrupt handler
* If you want to use these external low priority interrupts or any of the
* peripheral interrupts then you must enable them in your initialization
* routine.  Innovation First, Inc. will not provide support for using these
* interrupts, so be careful.  There is great potential for glitchy code if good
* interrupt programming practices are not followed.  Especially read p. 28 of
* the "MPLAB(R) C18 C Compiler User's Guide" for information on context saving.
* CALLED FROM:   this file, InterruptVectorLow routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
#pragma code
#pragma interruptlow InterruptHandlerLow save=PROD,section(".tmpdata")

void InterruptHandlerLow ()     
{
	if (PIR1bits.RC1IF && PIE1bits.RC1IE) // rx1 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_ONE_RX
		Rx_1_Int_Handler(); // call the rx1 interrupt handler (in serial_ports.c)
		#endif
	}                              
	else if (PIR3bits.RC2IF && PIE3bits.RC2IE) // rx2 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_TWO_RX
		Rx_2_Int_Handler(); // call the rx2 interrupt handler (in serial_ports.c)
		#endif
	} 
	else if (PIR1bits.TX1IF && PIE1bits.TX1IE) // tx1 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_ONE_TX
		Tx_1_Int_Handler(); // call the tx1 interrupt handler (in serial_ports.c)
		#endif
	}                              
	else if (PIR3bits.TX2IF && PIE3bits.TX2IE) // tx2 interrupt?
	{
		#ifdef ENABLE_SERIAL_PORT_TWO_TX
		Tx_2_Int_Handler(); // call the tx2 interrupt handler (in serial_ports.c)
		#endif
	}



//  ***  IFI Code Starts Here***
//                              
//  unsigned char int_byte;       
//  if (INTCON3bits.INT2IF && INTCON3bits.INT2IE)       /* The INT2 pin is RB2/DIG I/O 1. */
//  { 
//    INTCON3bits.INT2IF = 0;
//  }
//  else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE)  /* The INT3 pin is RB3/DIG I/O 2. */
//  {
//    INTCON3bits.INT3IF = 0;
//  }
//  else if (INTCONbits.RBIF && INTCONbits.RBIE)  /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
//  {
//    int_byte = PORTB;          /* You must read or write to PORTB */
//    INTCONbits.RBIF = 0;     /*     and clear the interrupt flag         */
//  }                                        /*     to clear the interrupt condition.  */
//  else
//  { 
//    CheckUartInts();    /* For Dynamic Debug Tool or buffered printf features. */
//  }
}


/*******************************************************************************
* FUNCTION NAME: User_Autonomous_Code
* PURPOSE:       Execute user's code during autonomous robot operation.
* You should modify this routine by adding code which you wish to run in
* autonomous mode.  It will be executed every program loop, and not
* wait for or use any data from the Operator Interface.
* CALLED FROM:   main.c file, main() routine when in Autonomous mode
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void User_Autonomous_Code(void)
{
  /* Initialize all PWMs and Relays when entering Autonomous mode, or else it
     will be stuck with the last values mapped from the joysticks.  Remember, 
     even when Disabled it is reading inputs from the Operator Interface. 
  */
  pwm01 = pwm02 = pwm03 = pwm04 = pwm05 = pwm06 = pwm07 = pwm08 = 127;
  pwm09 = pwm10 = pwm11 = pwm12 = pwm13 = pwm14 = pwm15 = pwm16 = 127;
  relay1_fwd = relay1_rev = relay2_fwd = relay2_rev = 0;
  relay3_fwd = relay3_rev = relay4_fwd = relay4_rev = 0;
  relay5_fwd = relay5_rev = relay6_fwd = relay6_rev = 0;
  relay7_fwd = relay7_rev = relay8_fwd = relay8_rev = 0;

  while (autonomous_mode)   /* DO NOT CHANGE! */
  {
    if (statusflag.NEW_SPI_DATA)      /* 26.2ms loop area */
    {
        Getdata(&rxdata);   /* DO NOT DELETE, or you will be stuck here forever! */
	relay5_fwd = 1;
        /* Add your own autonomous code here. */
		Camera_Handler();
		Servo_Track();
		Get_Tracking_State();
		gyro_raw = Get_Analog_Value(rc_ana_in01);
		GetCorrection();
		if (autostep == 0)
		{
			stop(4);
			printf("stopped %d \r", autostep);
			relay5_fwd = 1;
		}
		else if ( autostep == 1 )
		{
			//Camera_Handler();
			//Servo_Track();
			//Get_Tracking_State();
			drive();
			printf("driving%d   %d \r", correction, gyro_raw);
			relay5_fwd = 1;
		}
		else if ( autostep == 2 )
		{
			//Camera_Handler();
			//Servo_Track();
			//Get_Tracking_State();	
			Drive_To_Light();
			printf("driving%d \r", autostep);
			relay5_fwd = 1;
		}
		else if (autostep == 3)
		{
			stop(10);
			printf("stopped%d \r", autostep);
			relay5_fwd = 1;
		}
		else if (autostep == 4)
		{
			mastup(250);
			printf("mast%d \r", autostep);
			relay5_fwd = 1;
		}		
		else if(autostep == 5)
		{
			score();
			printf("score%d \r", autostep);
		}
		Generate_Pwms(pwm13,pwm14,pwm15,pwm16);
        Putdata(&txdata);   /* DO NOT DELETE, or you will get no PWM outputs! */
    }
  }
}

/*******************************************************************************
* FUNCTION NAME: Process_Data_From_Local_IO
* PURPOSE:       Execute user's realtime code.
* You should modify this routine by adding code which you wish to run fast.
* It will be executed every program loop, and not wait for fresh data 
* from the Operator Interface.
* CALLED FROM:   main.c
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
void Process_Data_From_Local_IO(void)
{
  /* Add code here that you want to be executed every program loop. */

}

/*******************************************************************************
* FUNCTION NAME: Serial_Char_Callback
* PURPOSE:       Interrupt handler for the TTL_PORT.
* CALLED FROM:   user_SerialDrv.c
* ARGUMENTS:     
*     Argument             Type    IO   Description
*     --------             ----    --   -----------
*     data        unsigned char    I    Data received from the TTL_PORT
* RETURNS:       void
*******************************************************************************/

void Serial_Char_Callback(unsigned char data)
{
  /* Add code to handle incomming data (remember, interrupts are still active) */
}


/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
